

import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;


public class FollowWall {

	static double SIDE_SIZE = 40.0;
	static double WHEEL_DIAMETER = 5.6f;
	static double AXIS_LENGTH = 5.6f;
	
	static double WALL_MINIMUM = 20;
	static double WALL_MAXIMUM = 20;
	public static void main (String[] args) throws Exception {
		
		// Navigator:
		DifferentialPilot navigator = new DifferentialPilot(WHEEL_DIAMETER, AXIS_LENGTH, Motor.A, Motor.C);
		
		// Instantiating Sensor ports:
		TouchSensor touchFront	= new TouchSensor(SensorPort.S1);
		TouchSensor touchBack 	= new TouchSensor(SensorPort.S2);
		UltrasonicSensor ultra	= new UltrasonicSensor(SensorPort.S3);
		
		// Calibrate sensors if needed:


		navigator.forward();
		while (true)
		{
			System.out.println(ultra.getDistance());
			
			if (Button.ESCAPE.isPressed()) {
				break;
			}
			
			
			if (touchFront.isPressed()) {
				navigator.stop();
				navigator.travel(-5);

				navigator.rotate(-45);
				navigator.forward();
			}
			// It follow the wall next to it, so it comes closer if needed:
			else if (ultra.getDistance() < WALL_MINIMUM) 
			{
				navigator.steer(-15, -15, true);
			
			// or it moves away if needed:
			} else if (ultra.getDistance() > WALL_MAXIMUM) 
			{
				navigator.steer(15, 15, true);
				
			} else 
			{
				// It keeps moving
				navigator.forward();
			}
		}
	}
}
